#include "CMatrix.h"
#include <iostream>
using std::cout;
using std::endl;

#include <math.h>
#include "CVector3f.h"


/*Constructor*/
CMatrix::CMatrix()
{
	for(int i = 0; i < 4; i++)
		for(int j = 0; j < 4; j++)
			m_array[i][j] = 0;

}

/*Destructor*/
CMatrix::~CMatrix(){}

/*Set the matrix to the identity*/
void CMatrix::identity()
{
	for(int i = 0; i < 4; i++)
		for(int j = 0; j < 4; j++)
		{
			if( i == j)
				m_array[i][j] = 1;
			else
				m_array[i][j] = 0;
		}
}

/*Calculate the inverse matrix*/
CMatrix CMatrix::inverse() const
{
	//the inverse set to the identity
	CMatrix I;
	I.identity();

	//the entry matrix
	CMatrix A;
	A = *this;
	
	//iterate through each line
	for(int k = 0; k < 4; k++)
	{
		//inverse line if a[k][k] = 0;
		if(A.m_array[k][k] == 0)
		{
			int i = 0;
			bool over = false;
			while(i < 4 && !over)
			{
				if(i == k)
				{
					i++;
					continue;
				}

				if(A.m_array[i][k] != 0)
				{
					over = true;

					//inverse row i and k
					for(int h = 0; h < 4; h++)
					{
						float temp = A.m_array[i][h];
						A.m_array[i][h] = A.m_array[k][h];
						A.m_array[k][h] = temp;

						temp = I.m_array[i][h];
						I.m_array[i][h] = I.m_array[k][h];
						I.m_array[k][h] = temp;
					}
				}
				i++;
			}
			//not inversible
			if(!over)
				return I;
		}

		//simplify the line to put a 1 in a[k][k]
		if(A.m_array[k][k] != 1)
		{
			float coeff = 1.f/A.m_array[k][k];
			for(int h = 0; h < 4; h++)
			{
				A.m_array[k][h] *= coeff;
				I.m_array[k][h] *= coeff;
			}
		}

		//pivot
		for(int i = 0; i < 4; i++)
		{
			if(i == k)
				continue;

			float coeff = A.m_array[i][k];

			for(int h = 0; h < 4; h++)
			{
				A.m_array[i][h] = A.m_array[i][h] - coeff * A.m_array[k][h];
				I.m_array[i][h] = I.m_array[i][h] - coeff * I.m_array[k][h];
			}
		}
	}

	return I;
}

/*display in std out*/
void CMatrix::display()const
{
	for(int i = 0; i < 4; i++)
	{
		for(int j = 0; j < 4; j++) 
			cout<<m_array[i][j]<<" ";

		cout<<endl;
	}			
}

/*addition*/
CMatrix CMatrix::operator+(const CMatrix& m)const
{
	CMatrix sum;
	for(int i = 0; i < 4; i++)
		for(int j = 0; j < 4; j++)
			sum.m_array[i][j] = m_array[i][j] + m.m_array[i][j];

	return sum;
}

/*substraction*/
CMatrix CMatrix::operator-(const CMatrix& m)const
{
	CMatrix res;
	for(int i = 0; i < 4; i++)
		for(int j = 0; j < 4; j++)
			res.m_array[i][j] = m_array[i][j] - m.m_array[i][j];

	return res;
}

/*multiplication*/
CMatrix CMatrix::operator*(const CMatrix& m)const
{
	CMatrix res;

	for(int i = 0; i < 4; i++) //row
	{
		for(int j = 0; j < 4; j++) //column
		{
			res.m_array[i][j] = 0;
			for(int h = 0; h < 4; h++) //offset
				res.m_array[i][j] += m_array[i][h] * m.m_array[h][j];
		}
	}

	return res;
}

/*multiplication*/
CVector3f CMatrix::operator*(const CVector3f& v)const
{
	CVector3f res;
	res.x = m_array[0][0] * v.x + m_array[0][1] * v.y + m_array[0][2] * v.z + m_array[0][3];
	res.y = m_array[1][0] * v.x + m_array[1][1] * v.y + m_array[1][2] * v.z + m_array[1][3];
	res.z = m_array[2][0] * v.x + m_array[2][1] * v.y + m_array[2][2] * v.z + m_array[2][3];

	return res;
}

/*add and assign*/
void CMatrix::operator+=(const CMatrix& m)
{
	for(int i = 0; i < 4; i++)
		for(int j = 0; j < 4; j++)
			m_array[i][j] = m_array[i][j] + m.m_array[i][j];
}

/*sub and assign*/
void CMatrix::operator-=(const CMatrix& m)
{
	for(int i = 0; i < 4; i++)
		for(int j = 0; j < 4; j++)
			m_array[i][j] = m_array[i][j] - m.m_array[i][j];
}

/*assign*/
void CMatrix::operator=(const CMatrix& m)const
{
	//copy array to array
	memcpy((void*)m_array, m.m_array, sizeof(float)*16);
}

/*create a translation matrix*/
void CMatrix::createTranslation(float x, float y , float z)
{
	identity();
	m_array[3][0] = x;
	m_array[3][1] = y;
	m_array[3][2] = z;
}

void CMatrix::createTranslation(const CVector3f& v)
{
	identity();
	m_array[3][0] = v.x;
	m_array[3][1] = v.y;
	m_array[3][2] = v.z;
}

/*create a rotation matrix around X axis*/
void CMatrix::createRotationX(float radiusAngle)
{
	identity();
	float c = cos(radiusAngle);
	float s = sin(radiusAngle);
	m_array[1][1] = c;
	m_array[1][2] = s;
	m_array[2][1] = -s;
	m_array[2][2] = c;
}

/*create a rotation matrix around Y axis*/
void CMatrix::createRotationY(float radiusAngle)
{
	identity();
	float c = cos(radiusAngle);
	float s = sin(radiusAngle);
	m_array[0][0] = c;
	m_array[0][2] = -s;
	m_array[2][0] = s;
	m_array[2][2] = c;
}

/*create a rotation matrix around Z axis*/
void CMatrix::createRotationZ(float radiusAngle)
{
	identity();
	float c = cos(radiusAngle);
	float s = sin(radiusAngle);
	m_array[0][0] = c;
	m_array[0][1] = s;
	m_array[1][0] = -s;
	m_array[1][1] = c;
}

/*create a rotation matrix a specific axis*/
void CMatrix::createRotation(const CVector3f& axis, float angle)
{
	float c = cos(angle);
	float s = sin(angle);

	float XPowerTwo = axis.x * axis.x;
	float YPowerTwo = axis.y * axis.y;
	float ZPowerTwo = axis.z * axis.z;

	m_array[0][0] = XPowerTwo + (1 - XPowerTwo) * c;
	m_array[0][1] = axis.x * axis.y * (1 - c) - axis.z * s;
	m_array[0][2] = axis.x * axis.z * (1 - c) + axis.y * s;
	m_array[0][3] = 0;
	m_array[1][0] = axis.x * axis.y * (1 - c) + axis.z * s;
	m_array[1][1] = YPowerTwo + (1 - YPowerTwo) * c;
	m_array[1][2] = axis.y * axis.z * (1 - c) - axis.x * s;
	m_array[1][3] = 0;
	m_array[2][0] = axis.x * axis.z * (1 - c) - axis.y * s;
	m_array[2][1] = axis.y * axis.z * (1 - c) + axis.x * s;
	m_array[2][2] = ZPowerTwo + (1 - ZPowerTwo) * c;
	m_array[2][3] = m_array[3][0] = m_array[3][1] = m_array[3][2] = 0;
	m_array[3][3] = 1;

}

/*create a non uniform scale matrix*/
void CMatrix::createScale(const CVector3f& s)
{
	identity();

	m_array[0][0] = s.x;
	m_array[1][1] = s.y;
	m_array[2][2] = s.z;
}

/*create a uniform scale matrix*/
void CMatrix::createScale(float s)
{
	identity();

	m_array[0][0] = s;
	m_array[1][1] = s;
	m_array[2][2] = s;
}

/*fill the matrix using opengl model of matrix*/
void CMatrix::getOpenGLMatrix(double* matrix)
{
	matrix[0] = m_array[0][0];
	matrix[1] = m_array[0][1];
	matrix[2] = m_array[0][2];
	matrix[3] = m_array[0][3];

	matrix[4] = m_array[1][0];
	matrix[5] = m_array[1][1];
	matrix[6] = m_array[1][2];
	matrix[7] = m_array[1][3];

	matrix[8] = m_array[2][0];
	matrix[9] = m_array[2][1];
	matrix[10] = m_array[2][2];
	matrix[11] = m_array[2][3];

	matrix[12] = m_array[3][0];
	matrix[13] = m_array[3][1];
	matrix[14] = m_array[3][2];
	matrix[15] = m_array[3][3];

}

void CMatrix::createFrenet(const CVector3f& position, const CVector3f& direction, const CVector3f& up)
{

	CVector3f normal = direction.cross(up);
	normal = normal * -1;
	normal.normalize();

	CVector3f binormal = direction.cross(normal);
	//binormal = binormal * -1;
	binormal.normalize();

	m_array[0][0] = normal.x;
	m_array[0][1] = normal.y;
	m_array[0][2] = normal.z;

	m_array[1][0] = binormal.x;
	m_array[1][1] = binormal.y;
	m_array[1][2] = binormal.z;

	m_array[2][0] = direction.x;
	m_array[2][1] = direction.y;
	m_array[2][2] = direction.z;

	m_array[3][0] = position.x;
	m_array[3][1] = position.y;
	m_array[3][2] = position.z;

	m_array[0][3] = m_array[1][3] = m_array[2][3] = 0;
	m_array[3][3] = 1;
}

//build a matrix using three vectors as columns
void CMatrix::buildColumn(const CVector3f& c1, const CVector3f& c2, const CVector3f& c3)
{
	m_array[0][0] = c1.x;
	m_array[1][0] = c1.y;
	m_array[2][0] = c1.z;
	m_array[3][0] = 0;

	m_array[0][1] = c2.x;
	m_array[1][1] = c2.y;
	m_array[2][1] = c2.z;
	m_array[3][1] = 0;

	m_array[0][2] = c3.x;
	m_array[1][2] = c3.y;
	m_array[2][2] = c3.z;
	m_array[3][2] = 0;

	m_array[0][3] = 0;
	m_array[1][3] = 0;
	m_array[2][3] = 0;
	m_array[3][3] = 1;
}

//calculate the determinant
float CMatrix::det()const
{
	float detM0 =	m_array[1][1]*m_array[2][2]*m_array[3][3] + 
					m_array[1][2]*m_array[2][3]*m_array[3][1] +
					m_array[1][3]*m_array[2][1]*m_array[3][2] -
					m_array[1][3]*m_array[2][2]*m_array[3][1] -
					m_array[2][3]*m_array[3][2]*m_array[1][1] -
					m_array[3][3]*m_array[1][2]*m_array[2][1];

	float detM1 =	m_array[1][0]*m_array[2][2]*m_array[3][3] + 
					m_array[2][0]*m_array[3][1]*m_array[1][3] +
					m_array[1][2]*m_array[2][3]*m_array[3][0] -
					m_array[1][3]*m_array[2][2]*m_array[3][0] -
					m_array[2][3]*m_array[3][1]*m_array[1][0] -
					m_array[3][3]*m_array[1][2]*m_array[2][0];

	float detM2 =	m_array[1][0]*m_array[2][1]*m_array[3][3] + 
					m_array[2][0]*m_array[3][1]*m_array[1][3] +
					m_array[1][1]*m_array[2][3]*m_array[3][0] -
					m_array[1][3]*m_array[2][1]*m_array[3][0] -
					m_array[2][3]*m_array[3][1]*m_array[1][0] -
					m_array[3][3]*m_array[1][1]*m_array[2][0];

	float detM3 =	m_array[1][0]*m_array[2][1]*m_array[3][2] + 
					m_array[2][0]*m_array[3][1]*m_array[1][2] +
					m_array[1][1]*m_array[2][2]*m_array[3][0] -
					m_array[1][2]*m_array[2][1]*m_array[3][0] -
					m_array[2][2]*m_array[3][1]*m_array[1][0] -
					m_array[3][2]*m_array[1][1]*m_array[2][0];

	return m_array[0][0]*detM0 - m_array[0][1]*detM1 + m_array[0][2]*detM2 - m_array[0][3]*detM3;
}